#ifndef URCONTROLOFFSET_HPP
#define URCONTROLOFFSET_HPP
#include "urcontrolarc.hpp"


class URControlOffset : public URControlArc
{
public:
    static cv::Vec3d HeadToBase(cv::Vec3d headCoordinate, cv::Vec6d transferHeadToBase)
    {
        cv::Matx33d rm;
        cv::Rodrigues(cv::Vec3d(transferHeadToBase[3], transferHeadToBase[4], transferHeadToBase[5]), rm);
        cv::Vec3d baseCoordinate=cv::Vec3d(transferHeadToBase[0], transferHeadToBase[1], transferHeadToBase[2]) + rm*headCoordinate;
        return baseCoordinate;
    }

    static cv::Vec4d HeadToBase(cv::Vec4d headPlane, cv::Vec6d transferHeadToBase)
    {
        cv::Vec3d headNormal(headPlane[0], headPlane[1], headPlane[2]);
        cv::Matx33d rm;
        cv::Rodrigues(cv::Vec3d(transferHeadToBase[3], transferHeadToBase[4], transferHeadToBase[5]), rm);

        std::cout<<"rm="<<rm<<"\n"<<std::flush;

        cv::Vec3d baseNormal=rm.inv().t()*headNormal;

        double baseD=headPlane[3]-baseNormal.ddot(cv::Vec3d(transferHeadToBase[0], transferHeadToBase[1], transferHeadToBase[2]));

        return cv::Vec4d(baseNormal[0], baseNormal[1], baseNormal[2], baseD);
    }

    static cv::Vec6d ReverseTransform(cv::Vec6d tf)
    {
        cv::Vec3d r(tf[3],tf[4],tf[5]);
        cv::Vec3d t(tf[0],tf[1],tf[2]);

        cv::Matx33d R;
        cv::Rodrigues(r,R);

        cv::Vec3d tnew=R.inv()*(-t);

        cv::Vec3d rnew;
        cv::Rodrigues(R.inv(),rnew);

        return cv::Vec6d(tnew[0],tnew[1],tnew[2],rnew[0],rnew[1],rnew[2]);
    }

    static cv::Vec6d ComposeTransform(cv::Vec6d tfFirst, cv::Vec6d tfSecond)
    {
         cv::Vec3d r1(tfFirst[3],tfFirst[4],tfFirst[5]);
        cv::Vec3d t1(tfFirst[0],tfFirst[1],tfFirst[2]);
        cv::Matx33d R1;
        cv::Rodrigues(r1,R1);

        cv::Vec3d r2(tfSecond[3],tfSecond[4],tfSecond[5]);
        cv::Vec3d t2(tfSecond[0],tfSecond[1],tfSecond[2]);
        cv::Matx33d R2;
        cv::Rodrigues(r2,R2);

        cv::Matx33d R=R2*R1;

        cv::Vec3d t=R2*t1+t2;

        cv::Vec3d r;
        cv::Rodrigues(R,r);

        return cv::Vec6d(t[0],t[1],t[2],r[0],r[1],r[2]);
    }


    static cv::Vec6d PointAtPlanePose(cv::Vec3d centroid, cv::Vec3d baseAxis[3], cv::Vec6d finger2Head, cv::Vec6d camera2Base)
    {
        cv::Vec3d ptCam[4]={
            centroid, centroid+baseAxis[0], centroid+baseAxis[1], centroid+baseAxis[2]
        };

        cv::Vec3d ptFinger[4]={
            cv::Vec3d(0,0,0),
            cv::Vec3d(1,0,0),
            cv::Vec3d(0,1,0),
            cv::Vec3d(0,0,1)
        };

        cv::Vec6d finger2Cam=ComputeTransform(ptFinger,ptCam);

        return ComposeTransform(ComposeTransform(ReverseTransform(finger2Head),finger2Cam),camera2Base);
    }

    static void XYVectorToXYZUnitVector(const cv::Vec3d oxy[3], cv::Vec3d oxyz[4])
    {
        oxyz[0]=oxy[0];
        oxyz[1]=oxy[1]-oxy[0];
        oxyz[1]/=cv::norm(oxyz[1]);
        oxyz[2]=oxy[2]-oxy[0];
        oxyz[2]/=cv::norm(oxyz[2]);
        oxyz[3]=oxyz[1].cross(oxyz[2]);
        oxyz[1]+=oxyz[0];
        oxyz[2]+=oxyz[0];
        oxyz[3]+=oxyz[0];
    }

//    chessboard coordinate to base coordinate
    static cv::Vec6d Calibrate()
    {
        cv::Vec3d newBase[4]={
            cv::Vec3d(0,0,0),
            cv::Vec3d(1,0,0),
            cv::Vec3d(0,1,0),
            cv::Vec3d(0,0,1)
        };

        cv::Vec6d finger2Head=cv::Vec6d(0,0,0.21,0,0,CV_PI/4);

        cv::Vec6d headPose[3]={
            cv::Vec6d(0.274599,-0.114707,0.223681,-2.91877,1.14276,-0.0202243),
            cv::Vec6d(0.474497,-0.104025,0.230525,-2.87844,1.23723,-0.110936),
            cv::Vec6d(0.262304,0.177009,0.222588,-2.88019,1.18513,-0.0505209)
        };

        cv::Vec3d oldoxy[3]={
            HeadToBase(cv::Vec3d(0,0,0), ComposeTransform(finger2Head, headPose[0])),
            HeadToBase(cv::Vec3d(0,0,0), ComposeTransform(finger2Head, headPose[1])),
            HeadToBase(cv::Vec3d(0,0,0), ComposeTransform(finger2Head, headPose[2]))
        };

        cv::Vec3d oldBase[4];
        XYVectorToXYZUnitVector(oldoxy,oldBase);
        return ComputeTransform(newBase, oldBase);
    }

    static cv::Vec6d ZPointerPose(cv::Vec3d centroid, double xyRoll, cv::Vec6d finger2Head, cv::Vec6d newBase2OldBase, bool zPlus=false)
    {
        cv::Vec3d baseAxis[3];
        baseAxis[2]=(zPlus?cv::Vec3d(0,0,1):cv::Vec3d(0,0,-1));
        baseAxis[0]=cv::Vec3d(cos(xyRoll),sin(xyRoll),0);
        baseAxis[1]=baseAxis[2].cross(baseAxis[0]);
        return PointAtPlanePose(centroid, baseAxis, finger2Head, newBase2OldBase);
    }

    static void ZPointer(cv::Vec3d centroid, double xyRoll, cv::Vec6d finger2Head, cv::Vec6d newBase2OldBase,
                         const char *ipUR, const char *ipPC, double a, double v, bool zPlus=false)
    {
        cv::Vec6d pose=ZPointerPose(centroid,xyRoll,finger2Head,newBase2OldBase,zPlus);
        URControl::SetTCPPose(ipUR,ipPC,pose[0],pose[1],pose[2],pose[3],pose[4],pose[5],a,v);
    }

    static void PickPoint(double openmm, double zOffset, cv::Vec3d centroid, double xyRoll, cv::Vec6d finger2Head, cv::Vec6d newBase2OldBase,
                          const char *ipUR, const char *ipPC, double a, double v, bool zPlus=false)
    {
        URControl::RG2(ipUR,ipPC,openmm);
        ZPointer(centroid, xyRoll, finger2Head, newBase2OldBase, ipUR, ipPC, a, v, zPlus);
        centroid[2]+=zOffset;
        ZPointer(centroid, xyRoll, finger2Head, newBase2OldBase, ipUR, ipPC, a, v, zPlus);
        URControl::RG2(ipUR,ipPC,0);
#if 1
        cv::Vec6d droppos(-0.0291873,-0.320623,0.363369,2.87222,1.23246,0.0494566);

        URControl::SetTCPPose(ipUR, ipPC, droppos[0],droppos[1],droppos[2],droppos[3],droppos[4],droppos[5], a, 0.5);

        URControl::RG2(ipUR, ipPC, openmm);
#endif
    }

public:
    cv::Vec6d baseOffset;
    URControlOffset()
    {
        baseOffset=Calibrate();
        std::cout<<baseOffset<<"\n"<<std::flush;
    }

//    z+ outside chessboard paper
    void ZPointer(double x, double y, double z, double xyRoll, double a, double v, bool zPlus)
    {
        char ipUR[]="192.168.1.13";
        char ipPC[]="192.168.1.14";
        ZPointer(cv::Vec3d(x,y,z),xyRoll,cv::Vec6d(0,0,0.21,0,0,CV_PI/4),baseOffset,ipUR,ipPC,a,v,zPlus);
    }

    void PickPoint(double openmm, double zOffset, double x, double y, double z, double xyRoll, double a, double v, bool zPlus)
    {
        char ipUR[]="192.168.1.13";
        char ipPC[]="192.168.1.14";
        PickPoint(openmm, zOffset, cv::Vec3d(x,y,z), xyRoll, cv::Vec6d(0,0,0.21,0,0,CV_PI/4),baseOffset,ipUR,ipPC,a,v,zPlus);
    }




};

#endif // URCONTROLOFFSET_HPP
